#ifndef BACKGROUNDPERPIXEL_H
#define BACKGROUNDPERPIXEL_H

#include "opencv2/core/core.hpp"

#include <stdexcept>
#include <algorithm>
#include <vector>
#include <stdio.h>

#include "BackGroundModel.h"

template< class T>
class BackGroundPerPixel : public BackGroundModel
{
public:
    BackGroundPerPixel( double containment , int minTolerance , int extraTolerance ,
                        int imgWidth , int imgHeight , int numChannels , int numFramesBG ) {

        this->containment = containment;
        this->minTolerance = minTolerance;
        this->extraTolerance = extraTolerance;
        this->imgWidth = imgWidth;
        this->imgHeight = imgHeight;
        this->numChannels = numChannels;
        this->numFramesBG = numFramesBG;
        this->totalElements = imgWidth*imgHeight*numChannels;

        this->values = new std::vector<int>[ totalElements ];
        this->tol = new int[ totalElements ];

        this->depth = sizeof(T);


        median = cv::Mat::zeros(imgHeight, imgWidth, CV_MAKETYPE((depth-1)*2,numChannels));

        reset();
    }

    virtual ~BackGroundPerPixel() {
        delete []values;
        delete []tol;
        tol = NULL;
        values = NULL;
    }

    virtual cv::Mat& getBackground() {
        return median;
    }

    virtual void reset() {
        for( int i = 0; i < totalElements; i++ ) {
            values[i].clear();
        }

        memset(tol,0,sizeof(T)*totalElements);
        memset(median.data,0,median.dataend-median.data);

        numSamples = 0;
    }

    virtual bool addImage( const cv::Mat &image ) {
        if( image.rows != imgHeight )
            throw std::runtime_error("Unexpected number of rows");
        if( image.cols != imgWidth )
            throw std::runtime_error("Unexpected number of columns");
        if( image.channels() != numChannels ) {
             printf("found = %d  expected %d\n",image.channels(),numChannels);
            throw std::runtime_error("Unexpected number of channels");
        }
        // open in opencv is setting the depth to 0
        if( image.depth() > 0 && image.depth() != depth ) {
            printf("image depth = %d  expected %d\n",image.depth(),depth);
            throw std::runtime_error("Unexpected depth");
        }

        const T *srcRowPtr = (const T *)image.datastart;

        std::vector<int> *dstPtr = values;

        for( int i = 0; i < imgHeight; i++ ) {
            const T *srcPtr = srcRowPtr;
            for( int j = 0; j < imgWidth; j++ ) {
                for( int k = 0; k < numChannels; k++ , dstPtr++ ) {
                    dstPtr->push_back( *srcPtr++ );

                }
            }
            srcRowPtr += image.step/depth;
        }

        printf("numSamples %d out of %d\n",numSamples,numFramesBG);

        return numSamples++ >= numFramesBG;
    }

    virtual void computeModel() {
        if( numSamples <= 0 )
            throw std::runtime_error("Must add at least one image.");

        printf("computeModel called\n");

//        computeMeanImage();

        int N = (int)(numSamples*containment);

        std::vector<int> errors;

        T *dstRowPtr = (T *)median.datastart;

        int index = 0;

        for( int i = 0; i < imgHeight; i++ ) {
            T *dstPtr = dstRowPtr;
            for( int j = 0; j < imgWidth; j++ ) {
                for( int k = 0; k < numChannels; k++ , index++ ) {
                    errors.clear();

                    std::vector<int> &v = values[index];
                    if( v.size() != numSamples )
                        throw std::runtime_error("EAGSDS");

                    // compute the median pixel value
                    sort (v.begin(),v.end());

                    int m = *dstPtr++ = v.at(numSamples/2);

                    // compute the errors now
                    for( int l = 0; l < v.size(); l++ ) {
                        errors.push_back(abs((int)v[l] - m));
                    }
                    sort (errors.begin(),errors.end());

                    int thresh = errors.at(N)+extraTolerance;
                    if( thresh < minTolerance )
                        tol[index] = minTolerance;
                    else
                        tol[index] = thresh;
                }
            }
            dstRowPtr += median.step/depth;
        }
    }


    virtual void findMovement( const cv::Mat &image , cv::Mat &movement  ) {
//        if( movement.depth() != 1 )
//            throw std::runtime_error("movement image should have a depth of 1");
//        if( movement.channels() != 1 )
//            throw std::runtime_error("movement image should have 1 channel");

        const T *srcRowPtr = (const T *)image.datastart;
        uint8_t *dstRowPtr = (uint8_t *)movement.datastart;
        const T *meanRowPtr = (const T *)median.datastart;

        int index = 0;

        for( int i = 0; i < imgHeight; i++ ) {
            const T *srcPtr = srcRowPtr;
            uint8_t *dstPtr = dstRowPtr;
            const T *meanPtr = meanRowPtr;

            for( int j = 0; j < imgWidth; j++ ) {
                bool bad = false;
                for( int k = 0; k < numChannels; k++ , index++ ) {
                    int error = abs( (int)*srcPtr++ - (int)*meanPtr++ );
                    bad |= error > tol[index];
                }
                if( bad ) {
                    *dstPtr++ = 255;
                } else {
                    *dstPtr++ = 0;
                }
            }
            srcRowPtr += image.step/depth;
            dstRowPtr += movement.step;
            meanRowPtr += median.step/depth;
        }
    }

protected:


    void shortPixels() {

    }

    double containment;

    int imgWidth;
    int imgHeight;
    int numChannels;
    int depth;

    // imgWidth*imgHeight*numChannels
    int totalElements;

    // number of images passed into addImage()
    int numSamples;

    //
    std::vector<int> *values;

    // image with mean values;
    cv::Mat median;
    // with the tolerance
    int *tol;
    // minimum allowed tolerance
    int minTolerance;
    // this is added to the computed tolerance
    int extraTolerance;

    // how many frames it should collect before computing the background model
    int numFramesBG;

};

#endif // BACKGROUNDPERPIXEL_H
